#ifndef GPS_H
#define GPS_H

// GPS library for the GPS 18x.

#include <stdbool.h>
#include "uavmath.h"

#define GPS_BUFFER_SIZE 128

typedef struct gps_session_t {
	int fd; // serial port file descriptor
	char rx_buffer[GPS_BUFFER_SIZE]; // buffer for reading from the serial port
	int rx_count; // number of characters in the buffer
} gps_session_t;

// GPS geodetic coordinate
typedef struct gps_coord_t {
	fixed_t lat; // latitude (radians)
	fixed_t lon; // longitude (radians)
	fixed_t alt; // altitude (meters)
} gps_coord_t;

// fix quality from GPGGA sentence
typedef enum gps_quality_t {
	GPS_QUALITY_NO_FIX = 0,
	GPS_QUALITY_NON_DIFFERENTIAL = 1,
	GPS_QUALITY_DIFFERENTIAL = 2,
	GPS_QUALITY_ESTIMATED = 6
} gps_quality_t;

#define GPS_INFO_NONE     0x0
#define GPS_INFO_POSITION 0x1
#define GPS_INFO_VELOCITY 0x2
#define GPS_INFO_BOTH     0x3

typedef struct gps_data_t {
	gps_coord_t coord;
	gps_quality_t quality;
	vector3_t velocity;
	int info; // = GPS_INFO_POSITION | GPS_INFO_VELOCITY if both a GPGGA and PGRMV have been read
} gps_data_t;

// Local reference frame
typedef struct ref_frame_t {
	gps_coord_t geodetic; // GPS geodetic coordinate of origin
	vector3_t ecef; // Earth Centered Earth Fixed coordinate of origin
	matrix_t ecef_to_enu; // transformation matrix for ECEF to local ENU conversion
} ref_frame_t;

// Opens the STUART (/dev/ttyS2) serial port to communicate with the GPS unit at the
// specified baud rate. The caller of this function is responsible for closing the session.
// Valid baud rates: B300, B600, B4800, B9600, B19200, B38400 (GPS 18x-5Hz only).
void gps_open_session(speed_t baud_rate, gps_session_t *session);

// Closes the serial port.
void gps_close_session(gps_session_t *session);

// Reads an NMEA sentence from the GPS unit into the provided buffer with maximum
// sentence length len, excluding null terminator (e.g., char buf[100]; len = 99).
// The sentence will be null-terminated. The delimiters ($ and \r\n) will not be included.
// Returns the sentence length (without null terminator) or -1 on failure.
int gps_read_sentence(gps_session_t *session, char *buf, int len);

// Convert degrees-minutes to radians.
fixed_t gps_deg_min_to_rad(int deg, fixed_t min);

// Gets the specified field from the sentence and copies into buf.
// The string copied into buf will be null terminated and of maximum length len.
// Field 0 is the sentence type.
// Returns the string lenght of the field copied into buf, or -1 on error.
int gps_get_field(const char *sentence, int field, char *buf, int len);

// Parse important data from the sentence.
// If the sentence is GPGGA, this function will set the coord and quality fields of the data struct.
// If the sentence is PGRMV, this function will set the velocity field.
// Returns the type of data acquired (GPS_INFO_x).
int gps_parse_sentence(const char *sentence, gps_data_t *data);

// Create a local ENU reference frame centered at the specified GPS geodetic coordinate.
void gps_create_ref_frame(const gps_coord_t *geo, ref_frame_t *ref);

// Convert a geodetic coordinate to an ECEF coordinate.
void gps_geodetic_to_ecef(const gps_coord_t *geo, vector3_t *ecef);

// Convert a geodetic coordinate (latitude, longitude, and altitude) to an ENU coordinate
// for the given local reference frame.
void gps_geodetic_to_enu(const ref_frame_t *ref, const gps_coord_t *geo, vector3_t *enu);

#endif
